﻿using System;
using System.Collections.Generic;
using Allegro.MathInterface;

namespace Allegro.Integrator
{
    public enum RombergIntegrationImplementation
    {
        Simple = 0,
        Mofa = 1,
    }
    /// <summary>
    /// Function integration using Romberg's integration method.
    /// See http://en.wikipedia.org/wiki/Romberg's_method
    /// In addition, the standard Mofa implementation has been ported to .net.
    /// </summary>
    public class RombergIntegrator : BaseIntegrator, IIterativeFunctionIntegrator
    {
        //const double EPS = 1.0e-6;
        private const int JMAX = 20;

        private RombergIntegrationImplementation _implementationUsed = RombergIntegrationImplementation.Simple;

        // Used by _SimpleImplementation
        private double[] s = new double[JMAX];
        private double[] powers = new double[JMAX];

        // Used by _MofaImplementation
        private double[] _row1;
        private double[] _row2;

        public RombergIntegrator()
        {
            _InitSimpleImplementation();
        }
        public RombergIntegrator(RombergIntegrationImplementation implementationUsed)
        {
            _implementationUsed = implementationUsed;
            switch (implementationUsed)
            {
                case RombergIntegrationImplementation.Simple:
                    _InitSimpleImplementation();
                    break;
                case RombergIntegrationImplementation.Mofa:
                    _InitMofaImplementation();
                    break;
            }
        }

        #region Implementation of IFunctionIntegrator

        /// <summary>
        /// Return the integral of the function f from a to b.
        /// </summary>
        /// <param name="f"></param>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public double Integrate(Func<double, double> f, double a, double b)
        {
            double result = double.NaN;
            switch (_implementationUsed)
            {
                case RombergIntegrationImplementation.Simple:
                    result = _SimpleImplementation(f, a, b);
                    break;
                case RombergIntegrationImplementation.Mofa:
                    result = _MofaImplementation(f, a, b);
                    break;
            }
            return result;
        }

        #endregion

        private void _InitSimpleImplementation()
        {
            powers[0] = 1.0;
            for (int i = 1; i < JMAX; i++)
                powers[i] = Math.Pow(4, i);
        }
        private void _InitMofaImplementation()
        {
            _row1 = new double[JMAX];
            _row2 = new double[JMAX];
        }
        private double _SimpleImplementation(Func<double, double> f, double a, double b)
        {
            int i, k;
            double tolerance = EPS;
            double os = double.NaN;
            for (i = 1; i < JMAX; i++)
                s[i] = double.NaN;
            for (k = 1; k < JMAX; k++)
            {
                for (i = 1; i <= k; i++)
                {
                    if (i == 1)
                    {
                        os = s[i];
                        s[i] = Trapezoid(f, a, b, k);     // sub-routine trapeze 
                    }                                   // integrated from 0 and 1
                    // pow() is the numbre of subdivisions.
                    else
                    {
                        //s[k] = (Math.Pow(4, i - 1) * s[i - 1] - os) / (Math.Pow(4, i - 1) - 1);
                        s[k] = (powers[i - 1] * s[i - 1] - os) / (powers[i - 1] - 1);

                        os = s[i];
                        s[i] = s[k];
                    }

                }
                if (Math.Abs(s[k] - s[k - 1]) < tolerance * Math.Abs(s[k - 1]) || (s[k] == 0.0 && s[k - 1] == 0.0))
                    return s[k];
            }
            throw new ApplicationException("Too many steps in routine RombergIntegrator.Integrate (default implementation)");
        }

        //double rombergIntegral(pFuncDouble func, const double& a, const double& b,
        //                  const double& error_limit)
        /// <summary>
        /// Port of the Mofa Romberg implementation
        /// </summary>
        /// <param name="func"></param>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        private double _MofaImplementation(Func<double, double> func, double a, double b)
        {
            // Calculate integral from a to b.
            // Note that the result can be bad if the function is not smooth or
            // it is constant over long intervals with peaks in between.  In
            // the last case, none of the peaks may be encountered by the 
            // subdivision of the first two (or more) consequtive iterations.

            if (a == b)
                return 0.0;

            double h = b - a;
            double error_limit = EPS;
            double[] v_row = _row1;
            double[] v_prev_row = _row2;
            //Vector<double> *v_row      = &row1;
            //Vector<double> *v_prev_row = &row2;

            v_row[0] = (h / 2.0) * (func(a) + func(b));

            double error = double.MaxValue;

            const int max_itr = JMAX;
            int i = 0;
            int powerOfTwo = 1;
            while (error > error_limit && i < max_itr)
            {
                ++i;
                powerOfTwo *= 2;
                //mfMath::swap(v_row, v_prev_row);
                double[] tmp = v_row;
                v_row = v_prev_row;
                v_prev_row = tmp;
                //v_row->resize(i+1);

                //double       *row      = &v_row     ->front();
                //const double *prev_row = &v_prev_row->front();

                h /= 2.0;
                double sum = 0;
                int end_loop = powerOfTwo - 1;//mfMath::pow(2, i)-1;
                for (int k = 1; k <= end_loop; k += 2)
                    sum += func(a + k * h);
                v_row[0] = v_prev_row[0] / 2 + sum * h;
                for (int j = 1; j <= i; ++j)
                    v_row[j] = v_row[j - 1] + (v_row[j - 1] - v_prev_row[j - 1]) / (Math.Pow(4, j) - 1);

                error = Math.Abs(v_row[i] - v_row[i - 1]);
            }
            if (i >= max_itr)
                throw new ApplicationException("Too many steps in routine RombergIntegrator.Integrate (Mofa implementation)");
                //return double.NaN;

            return v_row[i];
        }


    }
}
